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I.  INTRODUCTION 


The  goal  of  using  robot  manipulators  as  the  key  link  in 
flexible  automated  manufacturing  systems  has  presented 
engineers  with  a  variety  of  significant  problems.  For  a  six 
degree  of  freedom  robot,  the  position  and  orientation  of  the 
manipulator  end  point  must  be  specified  for  each  pose. 
Accuracy  and  repeatability  are  the  yardsticks  of  a  robot's 
performance.  Accuracy  is  the  measure  of  the  robot's  ability  to 
move  to  a  commanded  position  in  its  workspace.  Repeatability 
is  the  measure  of  the  robot  manipulator's  ability  to  return  to 
a  previously  learned  position.  Presently,  robots  that  are  used 
in  industrial  applications  display  adequate  repeatability,  but 
do  not  exhibit  satisfactory  levels  of  accuracy.  For  most 
industrial  robots,  repeatabilities  of  the  order  of  1  mm  or 
better  can  be  attained  while  the  positioning  accuracy  may  be 
off  by  as  much  as  1  cm  [Ref.  l:p.  14] .  For  on-line  programming 
applications  such  as  the  traditional  automated  pick  and  place 
operations  where  the  robot  manipulator  must  be  taught  the 
desired  motion,  adequate  repeatability  alone  is  sufficient. 
However,  as  the  concept  of  off-line  programming  was  developed 
as  a  means  of  automatically  generating  robot  control  programs 
for  tedious  applications  that  previously  would  have  involved 
large  numbers  of  taught  tasks,  the  low  levels  of  accuracy  that 
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robot  manipulators  could  attain  became  a  major  roadblock  to 
their  widespread  usage. 

There  are  several  factors  that  adversely  influence  the 
accuracy  of  robot  mianipulators .  Among  them  are:  temperature 
variations,  gear  backlash  and  harmonics,  compliance  in  links 
and  joints,  steady  state  errors  in  the  joint  servo 
controllers,  and  inaccurate  knowledge  of  the  manipulator's 
kinematic  parameters.  Experience  has  shown  that  the  most 
prevalent  source  of  error  is  inaccurate  knowledge  of  the 
kinematic  parameters  that  the  robot  controller  has  of  the 
manipulator  arm.  This  work  deals  primarily  with  the 
identification  of  the  variations  in  the  kinematic  parameters 
of  the  model  that  the  robot  controller  has. 

Even  small  variations  in  these  kinematic  parameters  can 
cause  significant  error  in  the  manipulator  end  point 
placement.  The  calibration  process  identifies  the  actual 
kinematic  parameters  of  the  model  and  uses  them  to  update  the 
robot  controller's  model  so  that  the  manipulator  end  point  may 
be  placed  into  a  commanded  position  with  greater  accuracy.  In 
calibration  tests  performied  by  Mooring,  Roth,  and  Driels  [Ref. 
2:p.  6]  and  several  others,  it  has  been  shown  that  correction 
of  the  kinematic  errors  resulted  in  improvement  in  accuracy  to 
the  same  order  of  magnitude  as  the  repeatability. 

The  process  of  robot  manipulator  calibration  is 
characterized  by  four  major  steps:  modelling,  measurement, 
identification,  and  correction.  The  first  step  in  the 
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calibration  process  is  to  form  a  valid  kinematic  model  of  the 
manipulator.  The  model  is  the  fundamental  relationship  between 
the  manipulator' s  kinematic  parameters  and  the  resulting  end 
effector  pose.  The  manipulator  model  may  take  two  basic  forms. 
The  forward  kinematic  model  is  used  to  compute  the  end 
effector  pose  given  the  joint  variable  data.  The  inverse 
kinematic  model  is  used  to  determine  the  joint  displacements 
for  a  given  pose.  The  kinem,atic  model  is  constructed  using  the 
Denavit-Hartenberg  method  with  modifications.  The  resulting 
model  is  used  to  define  an  error  quantity  based  on  the  nominal 
kinematic  parameter  set  and  the  unknown  actual  kinematic 
parameter  that  need  to  be  identified. 

Measurement  involved  physically  moving  the  manipulator  end 
effector  to  various  locations  in  its  workspace  and  recording 
the  corresponding  joint  displacements.  There  are  a  number  of 
methods  that  have  been  used  to  obtain  the  data  necessary  for 
manipulator  calibration.  Theodolites  [Ref.  3],  laser 
inferomieters  [Ref.  4],  coordinate  measuring  machines  [Ref.  5], 
and  many  other  techniques  can  be  used  depending  on  the 
constraints  imipcsed  by  the  desired  level  of  accuracy,  size, 
ease  of  use,  and  cost.  Alternatively,  joint  variable  data  and 
pose  infcrmaticn  can  be  obtained  through  computer  simulation 
with  the  use  of  a  random  number  generator  routine.  This  was 
the  approach  emipicyed  in  this  research. 

In  the  identification  phase,  the  task  is  to  identify  the 
set  of  model  param.eters  that  allow  the  poses  comiputed  from^  the 
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model  to  most  closely  match  the  measured  data.  This  is 
accomplished  through  the  use  of  a  gradient  based  Levenberg- 
Marquardt  algorithm  that  used  the  collected  pose  information 
to  ider.tify  the  actual  parameters  by  systematically  changing 
the  nominal  param.eters  to  reduce  the  previously  defined  error 
quantity.  There  are  several  factors  that  influence  the 
identification  process.  These  factors  are  the  type  of 
identification  routine  used,  the  initial  values  of  the 
parameters  to  be  determined,  the  number  of  poses  taken,  the 
influence  of  rr.^asurement  accuracy  and  noise,  encoder  noise, 
the  choice  of  measurement  configuration,  and  the  attainable 
range  of  joint  displacements  used  during  the  observations. 
These  effects  are  discussed  in  detail  at  a  later  time. 

Finally,  in  the  correction  step,  these  identified 
kinematic  param.eters  are  used  to  update  the  robot  controller's 
model.  This  process,  however,  is  not  without  its  own  unique 
set  of  problems.  Normally,  an  inverse  kinematic  so.lution  using 
the  actual  kinematic  parameters  is  employed  to  convert  the 
desired  off-line  locations  in  the  task  space  o  modified 
locations  in  the  manipulator's  own  joint  space.  The  robot  has 
an  inverse  kinematic  solution  for  the  nominal  model,  but  may 
have  to  develop  its  own  solution  using  the  actual  model.  These 
issues  are  beyond  the  scope  of  this  ''esearch  and  were  not 
addressed . 

The  purpose  of  this  research  was  to  com.pare  the  accuracy 
attained  for  two  different  computer  sim.ulated  calibration 
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methods.  The  first  method  involves  using  an  uncon.,  trained 
manipulator  end  point  and  the  second  method  employs  a  passive 
end  point  motion  constraint  callea  a  ballbar.  These  computer 
simulated  calibrations  were  performed  on  the  Model  G  Compact 
Master-Slave  Manipulator  shown  in  Figure  1.  The  Model  G 
Master-Slave  Manipulator  is  a  six  degree  of  freedom 
manipulator  arm  with  five  revolute  joints  and  one  prismatic 
joint  (5R1P) .  This  manipulator  is  designed  to  reproduce  the 
natural  miovements  and  force  of  the  human  hand.  The  manipulator 
end  point  will  move  in  exactly  the  same  manner  as  the  operator 
moves  the  manipulator  handle.  The  motion  is  constrained  only 
by  the  dimensional  limits  of  the  manipulator  itself.  The 
forces  produced  at  the  end  point  will  be  the  same  as  those 
forces  applied  at  the  handle  with  the  exception  of  minor 
losses  due  to  unbalance  and  friction.  This  manipulator  was 
chosan  for  these  calibrations  because  of  its  usefulness  for 
experim.ents  that  are  concerned  with  probing  of  objects  that 
can  not  be  viewed  during  the  probing  operation  to  acquire 
contact  information. 

The  format  of  the  remainder  of  this  thesis  will  be  to 
first  conduct  an  in-depth  examination  of  theory  applicable  to 
robot  calibration.  This  will  be  followed  by  an  analysis  of  the 
two  calibration  methods  used  and  the  unique  problem.s  with  each 
method.  Next,  wi'l  be  a  discussion  of  the  results  obtained 
anu,  finally,  the  conclusions  drawn  from,  this  research  will  be 
stated. 
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II.  THEORY 


A.  THE  DENAVIT-HARTENBERG  METHOD  WITH  MODIFICATIONS 

As  was  discussed  in  the  Introduction,  the  starting  point 
for  any  calibration  process  is  the  establishment  of  a 
representative  model  of  the  manipulator.  There  are  currently 
a  number  of  methods  of  generating  the  forward  kinematic  model 
of  a  serial  link  manipulator.  The  technique  that  was  used  to 
define  the  spatial  orientation  between  objects  and  various 
locations  in  the  manipulator  working  volume  is  the  Denavit- 
Hartenberg  method  [Ref.  6]  with  modifications  proposed  by 
Hayati  [Ref.  7],  Mooring  [Ref.  8],  and  Wu  [Ref.  9]  to  handle 
situations  in  which  consecutive  joint  axes  are  nominally 
patallel.  The  basic  concept  is  to  place  a  coordinate  frame  on 
each  of  the  manipulator  links  using  a  set  of  rules  that 
defines  the  origin  of  the  frame  and  its  orientation.  The 
position  of  consecutive  links  is  specified  by  a  homogeneous 
transformation  matrix,  which  transforms  the  frame  fixed  on 
link  n-1  into  the  frame  fixed  on  link  n.  This  transformation 
is  composed  of  more  fundamental  transformations  representing 
three  basic  translations  along  the  x,  y,  and  z  axes  and  three 
rotations  about  those  same  axes.  These  4x4  matrix 
transform.ations  are  expressed  as  follows: 
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cosB^  -sin02  0  0 
sinS^  COS02  0  0 
0  0  10 
0  0  0  1 

where  trans  (x,  y,  z)  describes  a  translation  given  by  the 
vector  r  =  [x,  y,  z]-  and  ROT  (x,  0^)  describes  a  rotation  of 
0^  about  the  x-axis  of  the  coordinate  frame. 

With  the  aid  of  Figure  2,  the  Denavit-Hartenberg 
transformation  methodology  can  be  illustrated.  First,  the  axis 
of  joint  motion  must  be  identified  and  the  z-axis  must  be 
aligned  with  the  axis  of  joint  motion.  Next,  the  common  normal 
between  consecutive  joint  axes  must  be  identified. 

Then,  the  origin  of  coordinate  frame  n  is  located  at  the 
intersection  of  joint  axis  n-f  1  and  the  common  normal  between 
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axes  n+1  and  n,  the  z  axis  of  coordinate  frame  n  is  always 
aligned  with  joint  axis  n+1  and  the  x  axis  is  always  aligned 
along  the  common  normal  between  consecutive  joint  axes. 

Transforming  frame  n-1  to  frame  n  is  accomplished  by  the 
following  sequential  steps: 

•  Rotate  frame  n-1  about  axis  z,.-  by  an  angle  9.,  the  joint 
angle . 

•  Translate  along  axis  2.,_  a  distance  d.,  the  offset. 

•  Translate  along  the  rotated  x._-  axis,  a  distance  a.,  the 
link  length. 

•  Rotate  about  axis  x.  by  an  angle  a,,  the  twist  angle. 

•  Rotate  about  axis  y.  by  an  angle  p. 

Incorporating  these  rules  with  the  transformation  matrix 
format  specified  in  Equations  1,  2  and  3,  the  transformation 
fromi  frame  n-1  to  frame  n  is  expressed  in  the  following  form: 

An  -  ROT  (  2,  e„  )  Trans  {  z.  d„)  Trans  (  x,  a„  )  ROT  (  x,  o„  )  ROT  (  y,  )  (5) 

Performing  the  matrix  multiplications  gives  the  resulting 
f  orm, : 

c0„cp„ -se„so„sp  -se„co„  ce„sp„-s0,so„cp„  a„c0; 

S0„CP„^C0„5O„SP„  C0„ca„  50„5P„  -  C0„SO„CP„  a„S0j  ^gj 
-Co„sP„  So„  ca„cp„  I 

0  0  0  1  J 

In  miost  cases,  four  out  of  the  five  transformiations  are 
necessary  to  transform,  frame  n-1  into  frame  n.  For  revolute 
joints,  the  parameters  dn,  a.  and  a.  are  constants  dictated  by 
the  geom.etry  of  the  manipulator  and  0.  is  the  angular  joint 
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variable.  The  parameter  P  is  defined  only  in  cases  where 
consecutive  coordinate  axes  are  parallel  and,  in  these 
instances,  d.,  is  normally  set  to  zero.  When  consecutive  axes 
are  parallel,  there  is  no  unique  common  normal.  The  P, 
parameter  allows  for  small  amount  of  inclination  between  the 
axes.  For  prismatic  joints,  the  location  of  the  origin  of  the 
coordinate  system  is  determined  by  extending  the  axis  so  that 
it  intersects  the  axis  of  the  next  joint.  This  makes  the 
length  of  the  common  normal,  a,,  and  the  next  joint  offset, 
d„.-,  both  equal  to  zero.  Therefore,  for  prism.atic  joints,  d_ 
is  the  joint  variable  and  the  link  geometry  is  described  by  6. 
and  a.  . 

In  order  for  a  robot  manipulator  to  have  comiplete 
dexterity  in  its  working  volume,  it  must  have  six  degrees  of 
freedom..  For  a  six  joint  six  link  manipulator,  the 
transformation  from  frame  5  to  frame  6  takes  the  form 

Ag  =  ROT  (  z,  4*6  )  rot  (  y,  06  )  ROT  (  x,  ilTg  )  Tians  ( p^,  p^,  p^g)  (7) 

where  the  rotations  are  sequentially  defined  as  roll,  pitch 
and  yaw  [Ref.  IC] .  The  transformation  from  the  base  coordinate 
frame  to  the  manipulator  end  link  is: 

Tg=  (8) 

Any  suitable  calibration  model  must  be,  in  Everett's  term.s 
[Ref  11],  complete.  Com.pleteness  refers  to  the  model's  ability 
to  relate  joint  displacements  to  the  tool  pose  for  a 
m.anipulator  while  allowing  for  the  arbitrary  placemient  of  the 
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world  coordinate  frame  and  arbitrary  assignment  of  the 
manipulator's  zero  position.  In  other  words,  the  model  must 
have  the  proper  number  of  identifiable  parameters  to  account 
for  variations  in  those  parameters.  The  required  number  of 
independent  kinematic  parameters  is  the  same  as  the  number  of 
constraint  equations  necessary  to  specify  the  tool  pose  and 
joint  frames.  Mooring,  Roth  and  Oriels  [Ref.  2:p.  43]  have 
concluded  that  for  each  revolute  joint,  four  independent 
kinem.atic  parameters  are  needed  and  for  each  prismatic  joint, 
two  independent  kinematic  parameters  are  required.  The 
required  number  of  independent  kinematic  parameters  N,  can  be 
determined  from  the  following  equation. 

W  =  4R  +  2P  +  6  (9) 

R  is  the  number  of  revolute  joints  and  P  is  the  number  of 
prismiatic  joints.  An  additional  six  parameters  are  specified 
in  crder  to  obtain  an  independent  tool  frame  location. 

B.  PARAMETER  IDENTIFICATIONS 

Tne  flowchart  in  Figure  3  outlines  the  calibration  process 
up  through  the  identification  step.  First,  the  range  of  motion 
for  each  joint  and  the  number  of  observations  to  be  made  must 
be  determined.  Next,  sets  of  joint  variables  for  each 
observat  io.ns  are  obtained  with  the  use  of  a  random  number 
generator  programi.  The  forward  kinemiatic  m.odel  of  the  5RiP 
m.anipulator  is  then  applied  to  generate  pose  data  for  each  of 
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Joint  Motion  Range 
Number  of  Observations 


Nominal  Parameters 
+  AP 


Generate  Joint  Variable  Data 

! 

Compute  Actual 

End  Effector  Position 

Figure  3 .  Flowchart  of  Calibration  Process . 
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the  observations  using  the  joint  variable  data  and  manipulator 
link  parameters.  The  kinematic  parameters  used  in  this  forward 
kinematic  solution  are  the  nominal  kinematic  parameters  plus 
a  known  error  parameter,  AP .  The  error  parameter  can  take  the 
form  of  a  length  error  or  an  angular  error  as  is  appropriate 
for  each  of  the  paramieters  that  is  to  be  identified.  The 
result  of  this  application  is  a  simulated  known  manipulator 
pose  for  each  of  the  joint  variable  sets.  The  random  noise  of 
measurement  and  encoder  noise  were  superimposed  on  the  pose 
data  and  joint  variables  respectively. 

The  simulated  observation  data  and  the  nominal  kinematic 
parameters  are  the  inputs  to  an  identification  program  ID6. 
ID6  initializes  the  nom.inal  kinematic  parameters  and  feeds 
them  to  an  identification  subroutine  ZXSSQ  which  numerically 
estimates  the  gradient  and  uses  it  to  produce  improved 
predictions  of  the  kinematic  model  parameters.  ZXSSQ  employs 
a  subroutine  that  takes  the  current  parameter  estimates  and 
calculates  an  error  between  the  model  predictions  and  the 
simulated  observation  data.  ZXSSQ  uses  the  error  to  determine 
the  gradient.  The  cycle  continues  until  convergence  criteria 
is  met.  The  parameter  estimation  is  treated  as  an 
unconstrained  non-linear  optimization  problemi.  Figure  4  shows 
a  flowchart  of  the  process. 

ZXSSQ  is  a  finite  difference,  Levenberg-Marouard^  routine 
that  is  tailored  for  non-linear  least  squares  problems  [Ref. 
2 : pp .  135-139] .  The  best  way  to  illustrate  its  use  is  through 
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Known  AP 
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i 
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Measured  Pose  Matrix  j 
Tm  i 


Estimated  Pose  Matrix 
T 


AT  -  T-TM 
Pose  Error 


ZXSSQ 

Charges  Unknown  AP 
To  Minimize  Pose  Error 


Figure  4.  Flowchart  of  Identification  Process 
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an  example.  Consider  the  simple  two  link  manipulator  in  Figure 
5.  What  values  of  0;  and  0,  will  put  the  manipulator  end  point 
at  point  Q?  If  P=F (0  ,  0,),  t  and  Cj  are  known  link  lengths, 
and  Q  is  a  known  position  in  the  two  dimensional  workspace, 
the  problem  is  to  find  0-  and  0;  such  that  the  quantity  z=  (P-Q) 
approaches  zero.  In  two-dimensional  matrix  notion,  the 
equations  are  as  follows: 


P.-Q. 

6  ■  cos  0-  f  -  cos  (  0-  +  0;  )  - 

(10) 

- 

t.sin0-  +  f-  sin  (0-  +  0-)  -  0.. 

Where  x^  and  are  the  difference  between  the  estimated  and 
known  parameters  in  the  x  and  y  directions  respectively.  The 
Levenberg-Marquardt  algorithm  uses  this  error  quantity  to 
numerically  estimate  the  gradient  and  produces  updated  0-  and 
0,;  values.  The  process  continues  until  predetermined 
convergence  criteria  are  met  and  0;  and  0  will  be  the  values 
needed  to  put  the  manipulator  end  point  at  point  Q.  A  more 
detailed  explanation  of  the  ZXSSQ  algorithm  is  given  in 
Appendix  A. 

According  to  Equation  9  for  the  5R1P  manipulator  used  in 
this  study,  it  would  be  expected  that  28  kinemiatic  parameters 
would  have  to  be  identified.  It  will  later  be  seen  that  this 
number  will  have  to  be  altered  Lo  meet  the  particular  needs  of 
the  measurement  method  employed  and  to  achieve  satisfactory 
parameter  identification. 
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III.  CALIBRATION  METHODS 


A.  UNCONSTRAINED  METHOD 

This  method  involves  establishing  a  valid  forward 
kinematic  model  using  the  Denavit-Hartenberg  methodology  and 
using  this  model  to  generate  the  manipulator  poses  that  will 
be  input  to  the  identification  program.  Employing  the  Denavit- 
Hartenberg  criter'a  previously  discussed  to  the  Model  G 
Master-Slave  Manipulator  produces  the  model  shown  in  Figure  6. 
The  location  of  the  world  coordir.ate  is  arbitrary  and  it  was 
fixed  in  the  position  shown  only  because  it  was  a  convenient 
reference  frame  for  measurements.  The  location  of  the  base 
fram.e  of  the  manipulator  is  also  arbitrary  as  long  as  is 
aligned  with  t^e  first  join'  axis.  The  remainder  of  the 
coordinate  frames  were  allocated  in  accordance  with  the 
Denavit-Hartenberg  Methuo.  Tne  t--^nsformation  from  frame  2  to 
frame  3  required  the  use  of  the  parameter  P,  because 
coordinate  frames  2  and  3  are  nominally  parallel  [Ref.  12]. 
The  definition  of  frame  6  is  arbitrary  and,  in  general,  the 
transformation  from  frame  5  to  frame  6  requires  three 
translations  and  three  rotations.  However,  since  frame  6  was 
chosen  to  be  offset  from  the  origin  of  coordinate  frame  5,  its 
orientation  is  undefined.  Therefore,  only  three  parameters  are 
required  to  transform  fri^m  frame  5  to  frame  6  and  at  least  one 
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of  these  parameters  must  be  a  displacement  otherwise  no 
movement  of  the  origin  of  frame  5  would  occur. 

The  parameters  4)^,  px.  and  pz,  were  chosen  to  define  the 
transformation  from  frame  5  to  frame  6  because  the  identified 
deviation  from  the  nominal  value  of  (j)^  is  composed  of  the 
encoder  offset  be,  and  the  constant  offset  S({)  f  as  well  as  the 
joint  variable  d,.  The  definition  of  0^  is  as  follows: 

0^  =  0, -80,  ^50,  (11) 

The  transformation  from  frame  5  to  frame  6  can  be  expressed  in 
a  reduced  form 

A,  =  ROT  (  z,  0,  )  TRANS  (  0,  )  (12) 

As  was  discussed  previously,  in  order  for  a  calibration 
model  to  be  valid,  it  must  satisfy  the  completeness  criteria 
specified  in  Equation  S.  The  required  number  of  identifiable, 
independent  kinematic  parameters  must  equal  the  number  of 
constraint  equations  needed  to  define  the  tool  pose  [Ref.  2.p. 
42].  Using  the  completeness  criteria,  it  is  expected  that 
there  must  be  28  identifiable  kinematic  parameters  in  order  to 
have  a  valid  model  of  a  5R1P  manipulator.  However,  because 
only  three  parameters  instead  of  six  were  used  to  specify  the 
transformation  from  frame  5  to  frame  6,  the  required  number  of 
independent  kinematic  parameters  is  25  for  this  calibration 
process . 

There  were  some  unique  problem.s  encountered  applying  the 
Denavit-Hartenberg  m.ethodology  to  the  Model  G  Master-Slave 
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Manipulator.  The  prismatic  joint,  in  particular,  requires  some 
modifications  to  the  Denavit-Hartenberg  model.  Because  the 
location  of  frame  3  is  defined  by  the  common  normal  between 
axes  4  and  5  and  the  prismatic  joint  axis  (frame  3)  is  not 
fixed  in  space,  the  prismatic  joint  axis  is  free  to  move  and 


• 

it  moves 

thro 

ugh 

the  origin  of  frame 

3 

[Ref.  l:p.  18]. 

Consequently, 

the 

parameters  aj  and  d^ 

are 

always  zero.  In 

addition, 

the 

parameter  G-  must  be  set 

at 

a  constant  value 

because  it  cannot  be  identified  independently  from  64  for  this 


manipulator  configuration.  Table  I  shows  the  table  of  the 
nominal  kinematic  parameters.  The  parameters  that  are  defined 


to  be  zero  are  in  boldface  type. 

TABLE  I.  NOMINAL  KINEMATIC  PARAMETER  TABLE 
(UNCONSTRAINED  CASE) 


50. 

d. 

aw 

a. 

Pw 

DEG 

mm 

mm 

DEG 

DEG 

0 . 0 

0.0 

1492.3 

90.0 

0.0 

Link  # 

50 

d 

a 

a 

P. 

DEG 

mm 

mm 

DEG 

DEG 

1 

0.0 

0.0 

0.0 

-90.0 

0.0 

2 

90 . 0 

0.0 

0.0 

-90.0 

0.0 

3 

0.0 

730.3 

0.0 

0 . 0 

0.0 

4 

-90 . 0 

0.0 

82 . 55 

90.0 

0.0 

5 

90 . 0 

0.0 

0.0 

90.0 

0.0 

0. 

PVt 

DEG 

DEG 

DEG 

mm 

0 

0 

0.0 

0.0 

50.8 

0.0 

50.8 

Having  established  a  valid,  working  model  of  the  Model  G 
Master-olave  Manipulator,  the  task  was  then  to  obtain  joint 
variable  data  for  a  variety  of  manipuictwr  poses.  This  was 


accomplished  using  Program  JOINT.  Program  JOINT  uses  a  random 
number  generator  subroutine  to  generate  the  joint  variable 
data  and  then  stores  it  in  a  data  file  called  TELE-VAR.DAT. 
Program  JOINT  is  run  a  second  time  to  obtain  joint  variable 
data  that  will  be  used  in  a  verification  program  that  will  be 
described  in  more  detail  later.  This  second  set  of  joint 
variable  data  is  stored  in  file  POSEVER.DAT. 

The  next  step  is  to  generate  pose  information  for  the 
Model  G  Manipulator  simulation.  Program  POSE  reads  the  joint 
variable  data  from  file  TELE-VAR.DAT  and  the  table  of  nominal 
kinematic  parameters  from  file  INPUT.DAT  and  computes  the 
manipulator  pose  using  a  forward  kinematic  solution.  The  set 
of  joint  variables  and  the  corresponding  manipulator  end  point 
pose  information  are  stored  in  file  TELE-POS.DAT.  In  program 
POSE,  estimates  of  measurement  noise  and  the  encoder  offsets 
are  added  to  the  data  through  INPUT.DAT. 

The  actual  kinematic  parameters  are  identified  by  program 
ID6,  using  the  previously  discussed  non-linear  least  squares 
method.  Program  ID6  consists  of  three  main  components.  The 
first  component  is  where  the  nominal  kinematic  parameters  are 
read  from  INPUT.DAT  and  the  pose  data  for  each  observation  are 
read  from  TELE-POSE.DAT.  The  program  then  defines  the  initial 
values  of  the  model  and  the  parameters  required  by  the 
identification  subroutine  ZXSSQ  are  initialized.  The 
identification  subroutine  ZXSSQ  is  the  second  major  component 
of  program  ID6.  The  details  of  how  ZXSSQ  works  can  be  found  in 
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the  parameter  identification  section  of  Chapter  II  and  in 
Appendix  A.  ZXSSQ  iteratively  estimates  the  gradient  and  uses 
the  estimate  to  produce  an  updated  approximation  of  the  model 
parameters.  The  cycle  continues  until  the  kinematic  parameters 
are  identified  consistently  to  four  significant  figures.  The 
third  major  component  of  program  ID6  is  the  subroutine  TELE¬ 
ARM  which  takes  the  current  estimate  of  the  model  parameters, 
computes  a  forward  kinematic  solution  using  the  estimated 
parameters,  and  then  calculates  the  error  between  the  model 
prediction  and  the  measured  pose  data.  This  error  is,  in  turn, 
used  by  ZXSSQ  to  determine  the  gradient.  The  output  of  program 
ID6  is  file  RESULT.DAT  which  consists  of  the  actual, 
identified  kinematic  parameters  of  the  manipulator  and  the 
calculated  RMS  difference  between  the  nomdnal  and  identified 
kinematic  parameters.  The  RMS  quantity  is  broken  down  into 
length  and  angular  error  parameters  (K.,  and  K, )  respectively. 
These  error  parameters  reflect  the  accuracy  of  the 
identification  process. 

The  final  stage  of  the  computer  simulated  calibration 
piocess  is  a  verification  program  designed  to  determ.ine  the 
accuracy  that  the  manipulator  could  attain  if  the  identified 
kinematic  parameters  were  to  replace  the  nominal  parameters  in 
the  m^anipulator '  s  controller.  Program  VERIFY  reads  the  nominal 
kinemiatic  parameters  from  INPUT.DAT  and  the  identified 
parameters  from  RESULT.DAT  and  computes  separately  for  each 
set  of  parameters  a  forward  kinematic  solution.  These 
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solutions  are  used  to  calculate  the  differential  position  and 
orientation  matrix. 


0  -5^  dx 

5,  0  -5^  dy 

-6,,  5  0  dz 

/  ^ 

_  0  0  0  0  _ 

The  position  error  is  calculated  as  follows: 


POSERR  =  \j  dy.‘  +dy^  +  dz^ 


(13) 


(14) 


The  position  error  is  indicative  of  the  accuracy  of  the 
calibrated  manipulator.  Figure  7  shows  a  flowchart  of  the 
programs  used  in  the  simulated  calibration  process.  These 
programs  and  data  files  can  be  reviewed  in  Appendix  B. 


B.  CONSTRAINED  (BALLBAR)  METHOD 

The  ballbar  method  involves  the  use  of  a  passive  end  point 
motion  constraint  to  obtain  pose  data.  The  end  point  of  the 
manipulator  is  connected  to  a  fixed  point  on  a  table  by  means 
of  a  ballbar  of  known  length.  Figure  8  shows  the  Model  G 
Master-Slave  Manipulator  configuration  with  the  ballbar  of 
length  552.8  mm  attached.  This  ballbar  length  was  obtained  by 
fixing  the  ballbar  at  a  location  very  near  the  manipulator  end 
point  when  it  is  in  the  zero  position.  The  other  end  of  the 
bar  was  connected  to  the  manipulator  end  point.  The  ballbar 
was  then  cycled  through  its  reachable  volume  while  constrained 
by  the  dimensional  limits  of  the  m.anipulator .  This  approximiate 
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Figure  7 .  Programs  Used  in  Unconstrained  Calibration  Process 
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bar  length  was  chosen  because  it  maximized  the  possible  range 
of  joint  displacements. 

The  coordinate  frames  were  allocated  in  accordance  with 
the  Denavit-Hartenberg  criteria.  With  the  exception  of  the 
world  coordinate  frame,  the  coordinate  frames  were  positioned 
exactly  as  they  were  for  the  unconstrained  calibration. 
However,  special  consideration  must  be  given  to  the  choice  of 
world  coordinate  frame.  If  it  is  assumed  that  all  the  Model  G 
Manipulator  joint  displacements  are  fixed,  a  rotation  of  frame 
0  relative  to  the  world  frame  results  in  a  change  of  the 
manipulator  end  point  (the  origin  of  frame  6)  coordinates  in 
the  x^,  y^,,  z,.  frame.  The  distance  between  the  world  coordinate 
frame  and  manipulator  end  point  is  the  fixed  length  of  the 
ballbar  and  it  remains  unchanged.  Since  the  relative  rotation 
between  the  world  frame  and  the  base  frame  cannot  be  measured 
the  conclusion  is  that  this  rotation  cannot  be  identified. 
Hcjnce,  it  xs  logical  that  the  world  coordinate  frame  selected 
be  orthogonal  to  the  base  frame.  The  location  of  the  base 
frame  is  arbitrary  as  long  as  the  Z;  axis  is  aligned  with  the 
first  join*-  axis. 

The  transformation  from  the  world  coordinate  frame  to  the 
base  frame  is  a  function  of  the  parameters  x^,  y^,  and  z^.  only. 
Because  parameters  z^  and  d  are  measured  in  the  same 
direction,  they  cannot  both  be  identified.  Therefore,  the 
parameter  z„  is  set  to  zero.  The  transformation  from  the  world 
coordinate  fram.e  to  the  base  frame  can  be  expressed  as  follows 
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Figure  8 .  Denavit-Hartenberg  Model  of  the  Master-Slave 
Manipulator  (Ballbar  Method) 
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=  TRANS  (  y^,  0  ) 


(15) 


In  addition,  it  has  been  determined  the  parameters  x^,  and 
50;  are  not  independent.  The  parameter  56;  cannot  be  identified 
and  was  set  to  zero.  The  table  of  nominal  kinematic  parameters 
for  ball  method  calibration  of  the  Model  G  Manipulator  is 
shown  in  Table  II.  The  parameters  in  bold  face  type  were  not 
identified  in  the  calibration  process.  For  the  ballbar 
calibration  method,  there  are  22  kinematic  parameters  that 
must  be  identified. 


TABLE  II.  NOMINAL  KINEMATIC  PARAMETER  TABLE 
(CONSTRAINED  CASE) 


Xw 

mm 

Yv 

mm 

Zw 

mm 

It  42. 7 

-132 . 95 

0.0 

Link  # 

50 

d. 

a 

a 

1 

0.0 

0.0 

0.0 

-90.0 

0.0 

2 

90.0 

0.0 

0.0 

1 

O 

O 

0.0 

3 

0.0 

0.0 

0.0 

0 .00 

0.0 

4 

i 

o 

o 

0.0 

82.55 

90 . 0 

0.0 

5 

90 . 0 

o.c 

0.0 

90 . 0 

0.0 

80. 

6 

V 

px. 

PYt 

PZe 

DEG 

DEG 

DEG 

mm 

mm 

mm. 

0 . 0 

0.0 

0.0 

50 . 8 

0.0 

50 . 8 

As  in  the  unconstrained  calibration  process,  the  simulated 
ballbar  miethod  calibration  of  the  Model  G  Manipulator  is 
accomplished  with  a  series  of  computer  prcgrams.  A  flowchart 
of  the  programs  used  is  shown  in  Fiqurf='  .  Tic  .  .  r  .-r.ct  r. 


28 


29 


is  fixed  at  552.8  mm  and  at  each  end  of  the  bar  is  a  ball 


joint  capable  of  90'  of  solid  angle  rotation.  One  end  of  the 
bar  is  attached  to  a  fixed  point  in  space  and  the  other  end  is 
attached  to  the  end  flange  of  the  Model  G  Manipulator.  Pose 
information  is  created  in  program  TELEBAR  by  using  a  random 
number  generator  routine  to  generate  the  angles  that  the  bar 
makes  with  the  z,  and  a.xes  (Figure  9)  .  The  position  of  the 
ballbar  end  point  is 

[  X,  y,  z  ]  ■  =  R  [  0,  0,  0,  1]  -  (16) 
where  the  transformation  R  is  defined  as  follows 

R  =  ROT  (  z,  e  )  ROT  (  y,  <}>  )  TRANS  (  x,  r  )  (17) 

For  each  pose,  the  distance  d  from  the  manipulator  end 
point  to  the  origin  of  the  world  coordinate  frame  is 
calculated  using 

d  =  \/x'  +  y‘  +  z-  (18) 

where  x,  y,  and  z  are  the  coordinates  of  the  manipulator  end 
point  in  space  relative  to  the  world  coordinate  frame.  Program 
TELEBAR  employs  the  previously  discussed  non-linear  least 
squares  algorithm;  ZXSSQ  to  m.inimize  the  function 

F=\d-i]  (19) 

where  C  is  the  length  of  the  ballbar  subroutine  ZXSSQ 
iteratively  estimates  a  gradient  and  produces  an  approxim.at ion 
of  the  joint  displacem.ents  necessary  to  establish  the  current 
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pose.  ZXSSQ  uses  subroutine  TELE-ARM  to  compute  the  forward 
kinematic  solution  of  the  manipulator  using  the  current  values 
of  joint  displacemients .  TELE-ARM  calculates  which  is  used 
by  ZXSSQ  to  determ.ine  the  gradient.  The  cycle  continues  until 
the  joint  variables  for  the  pose  are  consistently  identified 
to  four  significant  figures.  The  joint  displacements  are 
stored  in  file  TELE-SOLN.DAT.  Program  TELEBAR  is  run  a  second 
time  to  generate  joint  variable  displacements  for  use  in  the 
verification  phase.  The  second  set  of  joint  variables  is 
stored  in  file  POSEVER.DAT. 

Program,  FORWARD  is  used  to  check  the  validity  of  the  joint 
variable  data  generated  by  program  TELEBAR.  Programi  FORWARD 
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computes  the  forward  kinematic  solution  for  the  Model  G 
Manipulator  for  each  set  of  joint  displacements.  The  distance 
d  from  the  manipulator  end  point  to  the  world  coordinate  frame 
is 


d-\f> 


(19) 


where  x,  y,  and  z  are  the  coordinates  of  the  manipulator  end 
point  relative  to  the  world  coordinate  frame.  If  d  equals  the 
length  of  the  ballbar,  the  corresponding  set  of  joint 
displacements  is  valid.  Program  FORWARD  is  not  a  part  of  the 
calibration  process,  but  it  is  an  expeditious  way  to  check  the 
joint  variable  data. 

As  in  the  unconstrained  calibration  process,  the 
identification  of  the  actual  kinematic  parameters  of  the 
manipulator  is  accomplished  by  progran,  ID6.  Program  ID6  reads 
the  nominal  kinematic  parameters  from  INPUT.DAT  and  the  pose 
information  from  TELE-SOLN . uAT .  The  actual  kinematic 
parameters  of  the  manipulator  are  stored  in  file  RESULT.DAT. 
Program  VERIFY  calculates  the  accuracy  of  the  manipulator 
using  the  actual  identified  kinematic  parameters  as  was 
explained  earlier.  These  computer  programs  are  shown  in 
Appendix  B. 
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IV.  DISCDSSION 


In  order  to  obtain  a  satisfactory  comparison  of  the  two 
calibration  methods,  a  number  of  computer  simulated 
calibrations  were  performed  on  each  conf iqurat ion .  In  these 
simulations,  the  independent  variables  were  the  number  of 
observations  taken  and  the  level  of  measurement  noise  present. 
The  dependent  variables  were  the  accuracy  of  parameter 
identification  and  the  manipulator  accuracy  using  the 
identified  kinemiatic  parameters.  The  results  are  shown  in 
Figure  11  through  Figure  18.  For  the  unconstrained 
configuration,  the  low  noise  value  was  set  at  0.1  mm^.  The 
accuracy  of  parameter  identification  and  the  position  error 
were  each  separately  plotted  against  the  number  of 
observations.  The  same  procedure  was  repeated  with  the 
measurement  noise  increased  ten  times  to  1.0  mm.  The  entire 
process  was  again  repeated  using  the  low  and  high  noise  levels 
for  the  ballbar  configuration. 

In  general,  accuracy  of  parameter  identification  increased 
and  the  position  error  decreased  as  the  number  of  observations 
increased  regardless  of  noise  level  and  calibration 
configuration  used.  For  the  low  noise  level,  the  calibrated 
m.anipulator  accuracy  is  of  the  same  order  of  magnitude  as  the 
attainable  repeatability  (0.15  mm)  for  this  type  of 
m.anipulator .  This  suggests  that  in  the  presence  of  a  readily 
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attained  low  level  of  measurement  noise  (0.1  mm)  the 
manipulator  accuracy  attained  using  the  ballbar  method  is 
roughly  equal  to  that  attained  using  the  unconstrained  method 
and  is,  in  fact,  quite  satisfactory.  In  addition,  the 
manipulator  accuracy  can  be  improved  by  increasing  the  number 
of  observations  taken  during  the  measurement  phase  of  the 
calibration.  There  reaches  a  point,  however,  when  making 
additional  observations  produces  no  marked  improvement  in 
manipulator  accuracy.  Table  III  is  a  table  of  the  nominal  and 
identified  kinematic  parameters  for  the  unconstrained 
calibration  using  low  noise  and  60  observations.  Table  IV 
shows  the  same  parameters  for  the  ballbar  calibration  using 
low  noise  and  55  observations. 

When  the  high  noise  level  (1.0  mm)  was  used,  the  position 
error  obtained  using  the  ballbar  method  was  unsatisfactory 
even  when  a  large  number  of  observations  were  made.  For  the 
unconstrained  case,  the  position  error  was  an  order  of 
magnitude  higher  than  it  was  using  the  low  noise  level.  The 
ability  to  obtain  high  manipulator  accuracy  is  directly 
dependent  on  the  effectiveness  with  which  noise  can  be 
eliminated  from  the  measurement  process.  In  actual 
calibrations,  the  reduction  of  measurement  noise  is  a  pivotal 
step  in  the  process. 

For  calibrations  of  serial  link  manipulators,  the  ballbar 
method  has  been  proven  to  be  a  convenient  alternative  to  the 
use  of  expensive  ancillary  measurement  equipment.  With  this  in 
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ce  11 .  Accuracy  of  Parameter  Identification/Low  Noise 


(Unconstrained  Method) . 


Figure  12 .  Manipulator  Accuracy/Low  Noise 
(Unconstrained  Method) . 
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POSITION  ERROR,  MM  S  ACCURACY  OF  IDENTIFICATION,  MM 


NUMBER  OF  OBSERVATIONS 

e  15.  Accuracy  of  Parameter  Identification/Noise  x  10 


(Unconstrained  Method) . 


20  38  so  88  80  88 

_  NUMBER  OF  OBSERVATIONS 


Figure  16.  Manipulator  Accuracy /Noise  x  10 
(Unconstrained  Method) . 
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POSITION  ERROR.  MM  'g  ACCURACY  OF  IDENTIFICATION,  MM 

2.5  5  ht  0  0.5  1 


6  17.  Accuracy  of  Parameter  Identification/Noise  x  10 
(Ballbar  Method) . 


Figure  18.  Manipulator  Accuracy /Noise  x  10 
(Ballbar  Method) . 
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TABLE  III.  NOMINAL  AND  IDENTIFIED  KINEMATIC  PARAMETERS  USING 
UNCONSTRAINED  METHOD  WITH  LOW  NOISE  AND  60  OBSERVATIONS 


PARAMETER 

NOMINAL  VALUE 

IDENTIFIED  VALUE 

66. 

0.0 

1.00093 

d. 

0.0 

0.26324 

a. 

1492.3 

1492.54560 

ct. 

90.0 

91.00015 

50 

0.0 

0.99907 

d: 

0.0 

0.28461 

a. 

0.0 

0.25271 

« 

-90.0 

-89.00049 

60: 

90.0 

90 . 99987 

d; 

0 . 0 

0.24319 

a. 

0.0 

0.25737 

-90.0 

-88.99892 

d. 

730.3 

730.55844 

a 

0.0 

1  .  00002 

3 

0.0 

0 . 99810 

50: 

-90.0 

-89.00627 

3.; 

82.55 

82.78718 

a 

90.0 

90.97766 

60 

90.0 

91 . 02502 

d- 

0.0 

0.26702 

a- 

0.0 

0.23813 

a. 

90.0 

91.04527 

60. 

0.0 

0.99364 

P>h 

50.8 

51 . 01315 

Pz. 

50.8 

51.04286 

hand,  an  effort  was  made  to  determine  which  world  coordinate 
frame  locations  yielded  the  smallest  manipulator  position 
error.  Referring  back  to  Figure  8,  the  x-coordinate  of  the 
world  coordinate  fram;e  which  is  the  distance  to  the  first 
joint  axis  was  set  at  each  of  the  following  three  values: 


d: 

0.0 

0.23486 

a 

0.0 

0.26105 

a 

-90.0 

-89.00081 

80, 

90.0 

91.00244 

d: 

0.0 

0 .31004 

a- 

0 . 0 

0.30212 

O; 

-90.0 

-89.00261 

d;. 

o 

o 

0.42076 

o 

• 

o 

1.00434 

P 

o 

o 

1.00388 

50, 

1 

O 

• 

o 

- 

34 

82.55 

a. 

90.0 

89.00484 
82 .81179 
90 . 98414 


54)f 

0.0 

1 .01387 

PXe 

50.8 

50 . 65428 

Pz, 

50.8 

50.65121 

at  each  node  of  a  five  by  five  y-z  grid  and  the  position  error 
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was  computed  at  each  location.  Figure  19,  Figure  20,  and 
Figure  21  illustrate  the  results.  The  location  of  the  world 
coordinate  fram.e  does  not  significantly  influence  the  position 
error  obtained  as  long  it  is  within  the  manipulators  working 
volume . 

The  ballbar  method  is  ideal  for  calibration  of  industrial 
robots  in  that  it  is  quick,  inexpensive,  and  simple  to 
perform,.  The  manipulator  can  be  calibrated  in  place  without 
the  use  of  expensive  measurement  and  the  ballbar  calibration 
method  can  produce  manipulator  accuracy  of  the  same  order  of 
magnitude  as  other  more  costly  and  tedious  methods. 
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V.  CONCLUSIONS 


In  general,  accuracy  of  parameter  identification  and 
manipulator  accuracy  increased  as  the  numb^^r  of 
observations  taken  increased. 

Accuracy  of  calibration  process  is  directly  related  to  the 
extent  that  measurement  noise  is  reduced. 

Ballbar  method  produces  manipulator  accuracy  of  the  same 
order  as  the  unconstrained  method  and  it  is  less 
expensive,  quicker,  and  easier  to  perform. 

The  location  of  the  ballbar  does  not  significantly 
influence  the  accuracy  of  the  calibration  as  long  as  it  is 
within  the  robot's  working  volume. 


APPENDIX  A:  ZXSSQ 


ZXSSQ  is  a  Levenberg-Marquardt  finite  difference  routine 
for  solving  non-linear  least  squares.  The  problem  can  be 
stated  as  follows: 

Given  M  non-linear  functions  F.,  F2,  F^  of  a  vector 

parameter  x,  minimize  over  x 

F-  (  X  )  ^  =  F,  (  X  )  2  +  .  .  .  +  (  X  ) 

where  x  =  (x-  ,  Xp,  .  .  . ,  x^)  is  a  vector  of  N  parameters  to  be 

estimated.  When  fitting  a  nonlinear  model  to  data,  the 
functions  F.  should  be  defined  as  follows: 

F..  (  X  )  =  y.  -  g-  (  x;  vM  i  =  1,  2,  .  .  .  ,  M 

where  Y,  is  the  i''  observation  of  the  dependent  variable 

V  =  (v,  vp, .  .  .  ,  v.f)  is  a  vector  containing 

the  i’"  )bservation  of  the  NV  independent  variables 

g  is  the  function  defining  the  .aon-linear  model 

ZXSSQ  is  based  on  a  modification  of  the  Levenberg-Marquardt 

algorithm  which  eliminates  the  need  for  explicit  derivatives. 

Let  x"  be  an  initial  estimate  of  x.  A  sequence  of 

approximations  to  the  minimum  point  is  generated  by 

x'^*-  =  X  "  -  [  a,  D.  +  Ji  F  (  x'  ) 

where  J.  is  the  numerical  Jacobian  matrix  evaluated  at  x' 

D.  is  a  diagonal  matrix  equal  to  the  diagonal  of 
a.  is  a  positive  scaling  factor 
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V’hen  forward  differences  are  used,  the  Jacobian  is  calculated 
by 

[  F,  (  x  +  h,u.)  -F,.  (  X)  ] 
hj  ■ 

where  u-  is  the  j’"  unit  vector 
h.  =  max(|x.|,  0.1)  eps^'" 

eps  is  the  relative  precision  of  floating  point 
arithmetic 

For  central  difference,  the  Jacobian  is  as  follows 

^  [  F^  (  X  +  h ,  u.  )  -  F^  (x-hM^)  ] 

Finally, 

J,..  =  J,  +  [  F  (  x-^*-  )  -  F  (  xM  -  J,  5  ]  5~ 
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APPENDIX  B;  COMPUTER  PROGRAMS 


^★★★★★**Jt*TkJ^********************»*******'*Tt******************************** 


PROGRAM  JOINT 

C  This  program  generates  the  joint  variable  data  for  the 
C  MODEL  G  manipulator  simulation  by  random  methods. 

INTEGER  I,  J,  K,  NOBS,  MAXNOBS 
PARAMETER  (MAXNOBS=360 ) 

REAL  Q (MAXNOBS, 6) ,  QMIN(6),  QMAX(6) 

COMMON  /Cl/  Q,  QMiAX,  QMIN 

DATA  QMIN/  -160.0,  -223.0,-52.0,  -110.0,  -100.0,  -266.0  / 
DATA  QMAX/  160.0,  43.0,  232.0,  170.0,  100.0,  266.0  / 

WRITE  (6,*)  'Volume  is  MAX-POSSIBLE' 

DATA  QMIN/  -180.0,  -180.0,  0.0,  -180.0,  -180.0,  -180.0  / 
DATA  QMAX/  180.0,  180.0,  762.0,  180.0,  180.0,  180.0  / 
WRITE  (6,*)  'Volume  is  FULL' 

DATA  QMIN/  -90.0,  -90.0,  -90.0,  -90.0,  -90.0,  -90.0  / 
DATA  QMAX/  90.0,  90.0,  90.0,  90.0,  90.0,  90.0  / 

WRITE  (6,*)  'Volume  is  HALF' 

DATA  QMIN/  -45.0,  -45.0,  -45.0,  -45.0,  -45.0,  -45.0  / 
DATA  QMAX/  45.0,  45.0,  45.0,  45.0,  45.0,  45.0  / 

WRITE  (6,*)  'Volume  is  QUARTER' 

C  Open  output  data  file 

OPEN  (18,  NAME=' TELE-VAR.DAT' ,  STATUS=' NEW' ) 

C  Input  number  of  observations  from  data  file 

OPEN  (19,  NAME=' INPUT .DAT' ,  STATUS=' OLD' ) 

DO  1=1,10 
READ  (19,*) 

ENDDO 

READ  (19,*)  NOBS 

WRITE (*,*)  'NOBS=',NOBS 
CLOSE  (19) 

C  Call  the  generation  routine 
CALL  MSPREAD  (NOBS) 

C  Save  the  joint  variable  data 


DO  II  =  1,  NOBS 

WRITE  (18,*)  Q(II, 1) ,Q(II,2> ,Q(II, 3) ,Q(II, 4) ,Q(II,  5)  ,Q(II,  6) 
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ENDBO 


CLOSE  (18) 

STOP 

END 

Q  ★★★*Tfc***7t**»:*:*r*********^r****:*t************lt****lt****>t'*r***********lt* 

SUBROUTINE  MSPREAD  (NOBS) 

C  Tills  subroutine  generates  the  joint  data  by  the  Monte  Carlo  method. 
C  The  six  joint  variables  are  generated  from  six  independant 
C  uniform  random  variables. 

INTEGER  I,  J,  K,  NOBS,  MAXNOBS 
PARAMETER  (MAXNOBS=360 ) 

REAL  Q (MAXNOBS, 6) ,  QMIN(6),  QMAX(6) 

INTEGER*4  ISEED 
REAL  MAGQ ( 6 ) , NUM 

COMMON  /Cl/  Q,  QMAX,  QMIN 

C  Get  the  random  seed 

WRITE  (6,*)  'Type  in  a  6-digit  random  number  seed' 

READ  (5,*)  ISEED 

C  Calculate  the  Scaling  factor  for  each  random  variable 
DO  I  =  1,  6 

MAGQ  (I)  =  QMAX (I) -QMIN  (I) 

ENDDO 

C  Generate  the  joint  data 

DO  J  =  1,  NOBS 
DO  I  =  1,  6 

CALL  RANDOM  (ISEED, NUM) 

Q(J,I)  =  QMIN (I)  +  MAGQ (I)  *  NUM 

ENDDO 

ENDDO 

RETURN 

END 

SUBROUTINE  RANDOM  (x, z) 

REAL  FM,  FX,  Z 
INTEGER  A,  X,  I,  M 
DATA  1/1/ 

IF  (  I  .EQ.  0  )  GO  TO  1000 
1  =  0 

M=  2  **  20 
FM=  M 

.-■  =  2**10  +  3 
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1000  X=  MOD(  A*X  ,M) 

FX=  X 
Z=  FX/  FM 

RETURN 

END 

Q  **********Tk*****»*****************»***»******************* 
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PROGRAM  POSE 


C  This  program  generates  the  pose  data  for  the  MODEL  G  manipulator 
C  simulation.  It  reads  the  joint  variable  data  from  file  TELE-VAR.DAT. 

INTEGER*4  ISEED 

REAL* 8  RNX,  RNY,  RNZ,  MAGNX,  MAGNl 
REAL*8  RNl,  RN2 ,  RN3,  RN4,  RN5,  RN6 
INTEGER  I,  J,  K,  NOBS,  MAXNOBS,  N 
PARAMETER  (MAXNOBS=3 6 0 ) 

REAL* 8  DANGLE,  DLENTH 
REAL* 8  PI 

PARAMETER  (P 1=3 . 1 4 1 5 92 65358 97 93 ) 

REAL*8  DTW,  DTI,  DT2 ,  DT3,  DT4,  DT5 

REAL*8  DDW,  DDl,  DD2,  DD3,  DD4,  DD5 

REAL*8  AAW,  AAl,  AA2 ,  AA3,  AA4,  AA5 

REAL* 8  ALW,  ALl,  AL2,  AL3,  AL4,  AL5 

REAL*8  BLW,  BLl,  BL2 ,  BL3,  BL4,  BL5 

REAL*8  DF6,  FI6,  TH6,  SI6,  PX6,  PY6,  PZ6,  D3 

REAL*8  THETAl,  THETA2,  THETA3,  THETA4,  THETA5,  THETA6 

REAL*8  THW,  THl,  TH2,  TH3,  TH4,  TH5 

REAL*8  TW(4,4),  Tl(4,4),  T2{4,4),  T3(4,4) 

REAL*8  T4(4,4),  T5(4,4),  T6(4,4),  TRPY(4,4),  TXYZ(4,4) 

REAL* 8  TIMAT(4,4),  T(4,4) 

C  Initialize  the  TIMAT  matrix  to  an  I  matrix: 

DATA  TIMAT/ 1, 0,0,0,0,1,0,0,0,0,1,0,0,0,0,17 

C  Get  the  random  number  seed 

WRITE  (6,*)  'Type  in  a  6-digit  random  number  seed' 

READ  (5,*)  ISEED 

C  Open  input  files  and  output  data  file 

OPEN  (8,  NAME=' TELE-VAR.DAT' ,  STATUS=' OLD' ) 

OPEN  (9,  NAME=' TELE-POS .DAT' ,  STATUS= ' NEW' ) 

OPEN  (10,NAME=' INPOT.DAT' ,  STATUS*' OLD' ) 

C  Input  parameters 

read  (10, * ) 

read  (10,*)  dtw, ddw, aaw, alw, blw 
read  (10,*)  dt 1 , ddl , aa 1 , a  1 1 , bll 
read  (10,*)  dt2, dd2, aa2, al2, bl2 
read  (10,*)  dt3, dd3, aa3, al3, bl3 
read  (10,*)  dt4, dd4, aa4, al4,bl4 
read  (10,*)  dt 5 , dd5 , aa5 , a  15 , bl5 
read  (10, * ) 

read  (10,*)  df 6, th6, si6, px6, py6, pz6 
read  (1C, * ) 

read  (10,*)  nobs ,  n,  dangle, dlenth, magnx, magn 1 

C  Add  encoder  Offsets: 

DTW  *  DTW  DANGLE 
DTI  =  DTI  +  DANGLE 


!  defined 


DT2  =  DT2  +  DANGLE 
DT3  =  DT3 
DT4  =  DT4  +  DANGLE 
DT5  =  DT5  +  DANGLE 

C  Set  link  parameters  fcr  the  manipulator: 


ALW 

=  ALW 

+ 

DANGLE 

ALl 

=  ALl 

i- 

DANGLE 

AL2 

=  AL2 

+ 

DANGLE 

AL3 

=  AL3 

-t- 

DANGLE 

AL4 

=  AL4 

DANGLE 

AL5 

=  AL5 

■i- 

DANGLE 

AAW 

=  AAW 

+ 

DLENTH 

AAl 

=  AAl 

+ 

DLENTH 

AA2 

=  AA2 

DLENTH 

AA3 

=  AA3 

!  defined 

AA4 

=  AA4 

DLENTH 

AA5 

=  AA5 

DLENTH 

DDW 

=  DDW 

DLENTH 

DDl 

=  DDl 

+ 

DLENTH 

DD2 

=  DD2 

DLENTH 

DD3 

=  DD3 

DLENTH 

DD4 

=  DD4 

!  defined 

DD5 

=  DD5 

DLENTH 

BLW 

=  BLW 

!  defined 

BLl 

=  BLl 

!  defined 

BL2 

=  BL2 

!  defined 

BL3 

=  BL3 

+ 

DANGLE 

BL4 

=  BL4 

!  defined 

BL5 

=  BL5 

!  defined 

DF6 

=  DF6 

+ 

DANGLE 

TH6  = 

0.0 

SI6  = 

0.0 

PX6  = 

PX6  4 

DLENTH 

py6  = 

C  .  0 

PZ6 

=  PZ6 

DLENTH 

D3  = 

DD3 

C  Loop  NOBS  times 

Do  I  =  1,  NOBS 

C  Initialize  the  T  matrix  to  an  I  m.atri:-:: 

DO  J=l, 4 
DC  K=1 , 4 

T(J,K)  =  TIMAT(J,K) 

ENDDC 

ENDDO 

C  Manipulator  joint  angle  input : 

READ  (8,*)  THETAl,  THETA2,  THETA3,  THETA4,  THETAS, 

THW  =  DTW 

THl  =  DTI  +  THETAl 


THETA 
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TH2  =  DT2  +  THETA2 
TH3  =  DT3 

TH4  =  DT4  +  THETA4 

TH5  =  DT5  +  THETAS 

FI6  =  DF6  +  THETA6 

DD3  =  D3  +  THETA3 

C  Compute  the  T  matrices,  TW  thru  T6: 

CALL  TRANSFORM  (  ALW,  AAW,  DDW,  THW,  BLW,  TW  ) 


CALL 

TRANSFORM  ( 

ALl, 

AAl, 

DDl, 

THl, 

BLl, 

T1 

CALL 

TRANSFORM  ( 

AL2, 

AA2, 

DD2, 

TH2, 

BL2, 

T2 

CALL 

TRANSFORM  ( 

AL3, 

AA3, 

DD3, 

TH3, 

BL3, 

T3 

CALL 

TRANSFORM  ( 

AL4, 

AA4, 

DD4, 

TH4, 

BL4, 

T4 

CALL 

TRANSFORM  ( 

AL5, 

AA5, 

DD5, 

TH5, 

BL5, 

T5 

CALL 

T3RPY  (  FI6, 

TH6, 

SI6, 

TRPY 

) 

CALL 

T3XYZ  (  PX6, 

PY6, 

PZ6, 

TXYZ 

) 

CALL  MATMULC  (  T6,  TRPY,  TXYZ  ) 

C  Comipute  the  overall  transformation,  T: 

CALL  MATMULA  (  T,  TW  ) 

CALL  MATMULA  (  T,  T1  ) 

CALL  MATMULA  (  T,  T2  ) 

CALL  MATMULA  (  T,  T3  ) 

CALL  MATMULA  (  T,  T4  ) 

CALL  MATMULA  (  T,  T5  ) 

CALL  MATMULA  (  T,  T6  ) 

C  Generate  the  random  noise 

CALL  RANDOM (ISEED,RNX) 

CALL  RANDOM (I SEED, RNY) 

CALL  RANDOM (I SEED, RNZ) 

CALL  RANDOM (ISEED,RN1) 

CALL  RANDOM (I SEED, RN2) 

CALL  RANDOM (ISEED,RN3) 

CALL  RANDOM (I SEED,  RN4) 

CALL  RANDOM ( ISEED, RN5) 

CALL  RANDOM ( ISEED,  RN6) 

RNX  =-  X1AGNX*(  2.0»RNX  -  1.0  ) 

RNY  =  MAGNX* (  2.0* RNY  -  1.0  ) 

RNZ  =  MAGNX* (  2.0* RNZ  -  1.0  ) 

RNl  =  MAGN1*(  2.0*RN1  -  1.0  ) 

RN2  =  MAGN1*(  2.0*RN2  -  1.0  ) 

RN3  =  MAGN1*(  2.0*RN3  -  1.0  ) 

RN4  =  MAGN1*(  2.0*F114  -  1.0  ) 

RN5  =  MAGNl *  (  2 . 0  *RN5  -1.0) 

RN6  =  MJ\GN1*(  2.C*RN6  -  1.0  ) 

C  Add  noise  to  mieasurem^nts  and  encoder  readings 

T(l,4)  =  T(l,4)  -  Ri:X 

T(2,4)  =  T(2,4)  -  RNY 

T(3,4)  =T(3,4)  ♦  RNZ 

THETAl  =  theta:  -RNl 


THETA2  =  THETA2  +RN2 
THETA3  =  THETA3  +RN3 
THETA4  =  THETA4  +RN4 
THETAS  =  THETAS  +RNS 
THETA6  =  THETA6  +RN6 

C  Store  the  manipulator  joint  vector  and  measured  tool  pose 

WRITE  (9,991)  THETAl,  THETA2,  THETA3,  THETA4,  THETAS,  THETA6 
WRITE  (9,992)  T(l,4) 

WRITE  (9,992)  T(2,4) 

WRITE  (9,992)  T(3,4) 

WRITE  (9,*) 

C  Format  below  decides  the  digits  of  accuracy  of  simulation  data 

991  FORMAT  (  6F12.6  )! Joint  vector  data 

992  FORMAT  (  F12.5  )  .'Measurement  data 

C  End  do-loop  for  counter  I 
ENDDO 

WRITE  (6,*)  'Data  stored  in  F12.S,  F12.4  format' 

CLOSE  (8) 

CLOSE  (9) 

STOP 

END 

SUBROUTINE  RANDOM  (x,z' 

REAL  FM,  FX,  Z 
INTEGER  A,  X,  I,  M 
DATA  1/1/ 

IF  (  I  .EQ.  0  )  GO  TO  1000 
1  =  0 

M=  2  **  20 
FM=  M 

A=  2**10  *  3 

1000  X=  MOD (  A*X  ,M) 

FX=  X 
Z=  FX/  FM 

RETURN 

END 
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PROGRAM  TELEBAR 


C  This  program  generates  a  set  of  joint  angles  for  the  calibration 
C  of  the  MODEL  G  manipulator  using  a  ball  bar  to  constrain  the  end 
C  point  of  the  manipulator. 


INTEGER  LDFJAC,  M,  N,  obs,  nobs 
PARAMETER  (LDFJAC=3.  M=LDFJAC.  N=6) 


REAL *8 

DTI, 

DT2, 

DT3, 

REAL *8 

DDl, 

DD2, 

DD3, 

REAL*8 

AAl, 

AA2, 

AA3, 

REAL *8 

ALl, 

AL2, 

AL3, 

REAL *8 

BLl, 

BL2, 

BL3, 

REAL*8 

DF6, 

FI6, 

TH6, 

REAL *8 

xw. 

YW,  ZW 

DT4,  DT5 
DD4,  DD5 
AA4,  AA5 
AL4,  AL5 
BL4,  BL5 

SI6,  PX6,  PY6,  PZ6 


INTEGER  infer, ier, iopt, nsig,maxfn 

REAL*8  FJAC (LDFJAC, N) ,  x jt j ( (n+1 ) *n/2 ) ,  x jac ( Idf jac , n) 
REAL*8  parm(4),  fildfjac),  work ( (5*n) + (2*m) +( (n+1) *n/2) ) 
REAL»8  X(N) 

real *8  r,  phimax,  phim.in,  thetamax,  thetamin,  phi,  theta 
real  *  8  xb, yb, zb, ssq, r r , magnx, magnl 


EXTERNAL  TELE  ARM 


INTEGER  I,  J,  K 

REAL*8  TDES(4,4),  qmax(6),  qmin(6),  SCALE,  DANGLE,  DLENTH,  NUM 
COMMON  /PDATA/  TDES,  DANGLE,  DLENTH,  r 
COMMON  /KIN/  DT1,DT2,DT3,DT4,DT5, 

&  AL1,AL2, AL3, AL4, AL5, 

&  AA1,AA2, AA3, AA4, AA5, 

i  DD1,DD2,DD3,DD4,DD5, 

4  BLl, BL2, 3L3, BL4, BL5, 

&  XW,YW, ZW, 

&  DF6, THo, SI6, PX6, PY6, PZ6 

C  Joint  angle  ranges 

data  qm.in/-3  0  .0,-45.0,0.0,-180.0,0.0,-180.0/ 
data  qm, ax/25.0,  45.0,  762.0,  180.0,  90.0,  180.0/ 


C  Initialize  data  variables 


obs  =  0 


C  Open  data  files  for  input 

OPEN  (10,  NAME=' TELE-SOLN.DAT' ,  STATUS= ' NEW' ) 
open  (9,  NAME=' INPUT .DAT' ,  STATUS=' old' ) 

C  Read  input  kinematic  data 


read 

(9,  *) 

read 

(9,  M 

xw, yw, ZW 

read 

(9,  *) 

dtl,ddl,aal,all,bll 

read 

(9,  M 

at  2 , dd2 , aa2,al2,bi2 

read 

(9,  *) 

dt3,dd3,aa3,al3,bl3 

read 

(9,  *) 

dt  4 , dd4 ,aa4,al4,bl4 
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read  (9,*)  dt5,dd5, aa5, al5,bl5 
read  ( 9, * ) 

read  (9,*)  df 6, th6, si6, px6, py6, pz6 
read  ( 9, * ) 

read  (9,*)  nobs,  r,  dangle, dlenth,magnx, magnl 


close  (9) 

C  Adjust  nominal  values 

xw=xw+dlenth 

yw=yw+dlenth 

dt2=dt2+dangle 

dt4=dt4+dangle 

dt5=dt5+dangle 

all=all+dangle 

al2=al2+dangle 

al3=al3+dangle 

al4=al4+dangle 

al5=al5+dangle 

aal=aal+dlenth 
aa2=aa2  +dlenth 
aa4=aa4+dlenth 
aa5=aa5+dlenth 

ddl=ddl+dlenth 

dd2=dd2+dlenth 

dd3=dd3+dlenth 

dd5=dd5+dlenth 

bl3=bl3+dangle 

df  6=df  6+dangle 
px6=px6-fdlenth 
pz6=pz6+dlenth 

C  Limits  on  bar  rotation 

phimax=l 8  0 . 0 
phimin=-180 . 0 
thetamax=90 . 0 
thetamin=-90 . 0 

C  Get  random  number  seed 

c  ISEED  =  123456 

write  (6,*)  'Type  in  a  6-digit  random  number  seed' 
read  ( 5 , * )  iseed 

C  Write  NOBS  to  TELE-SOLN.DAT 

write  (10,*)  nobs 

C  Start  of  main  loop 
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C  Set  joint  angles  to  zero 

do  i=l,n 
X (i) =0  .  C 
enddo 

C  Get  random  bar  angles 

1000  call  random  (iseed,num) 

phi=phimin+ (phimax-phimin) *num 
call  random  (iseed,num) 

theta=thetamin+ ( thetamax-thetamin) *num 

C  Calculate  end  point  of  the  bar 

xb=r*cosd (theta) 
yb=r*sind (theta) *cosd(phi) 
zb=r*sind (theta) *sind(phi) 

C  Reacheability  calculation 

if  (z  .It.  0.0)  go  to  1000 


C  Establish  desired  tool  pose 

do  ii=l , 4 
do  jj=l,4 

TDESdi,  jj)=0.0 
enddo 
enddo 

TOES (1, 4) =xb 
TOES (2, 4) =yb 
TOES (3, 4) =zb 
TDES(4,4)  =  1.0 

C  Call  IMSL  ZXSSQ  for  inverse  )cinematic  solution 


nsig=4 
eps=0  . 0 
delta=0 . 0 
maxfn=500 
iopt=l 

ix jac  =  ldf  jac 

CALL  ZXSSQ  ( tele_arm,  m,  n,  ns ig,  eps,  delta ,  maxfn,  iopt ,  parmi,  x, 
&  ssq,f,xjac,ixjac,xjtj,  wo  r'K,  infer,  ier) 


C  Print  results  to  2  decimal  places 
writevD,*)  obs , ssq, iseed 

WRITE  (10,*)  XC),  X(2),  X(3),  X(4),  X(5),  X(6) 
C  Continue  for  ether  bar  angles 

if  (obs  .It.  nobs)  go  to  ICIO 
CLOSE  (10) 
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WRITE (6,*)  XW,YW,ZW 

END 

(2  *************************************************^******************* 
SUBROUTINE  tele_ARM  (X,M,N,F) 

C  This  subroutine  calculates  the  non-linear  function  for  the  use  of 
C  the  IMSL  routine  ZXSSQ.  It  is  the  forward  kinematic  solution  for 
C  the  MODEL  G  manipulator. 

INTEGER  M,  N 
REAL*8  X(N),  F(M) 

INTEGER  II,  JJ 

REAL*8  DTI,  DT2,  DT3,  DT4,  DT5 

REAL*8  DDl,  DD2,  DD3,  DD4,  DD5 

REAL*  8  AAl,  AA2 ,  AA3,  AA4,  AA5 

REAL*8  ALl,  AL2 ,  AL3,  AL4,  AL5 

REAL*8  BLl,  BL2,  BL3,  BL4,  BL5 

REAL*8  DF6,  FI6,  TH6,  SI6,  PX6,  PY6,  PZ6 

REAL* 8  XW,  YW,  ZW,  D3 

REAL*8  THl,  TH2,  TH3,  TH4,  TH5 

REAL*8  T0(4,4),  Tl(4,4),  T2(4,4),  T3(4,4),  T4(4,4) 

REAL*8  T5(4,4),  T6(4,4),  trpy(4,4),  txyz(4,4) 

REAL*8  TIMAT(4,4),  T(4,4) 

REAL*8  disq,dis 

INTEGER  I,  J,  K 

REAL*8  TDES(4,4),  DANGLE,  DLENTH,  r 

COMMON  /PDATA/  TDES,  DANGLE,  DLENTH,  r 
COMMON  /KIN/  DT1,DT2,DT3,DT4,DT5, 

5  ALl, AL2, AL3,AL4,AL5, 

6  AAl, AA2, AA3,AA4, AA5, 

&  DD1,DD2,DD3,DD4,DD5, 

&  BL1,BL2,BL3,BL4,BL5, 

&  XW,YW,ZW, 

&  DF6, TH6, SI6, PX6, PY6, PZ6 

C  Initialize  the  TIMAT  matrix  to  an  I  matrix: 

DATA  TIMAT/ 1, 0,0, 0,0, 1,0, 0,0, 0,1, 0,0, 0,0,1/ 

C  Initialize  the  T  matrix  to  an  I  matrix 

DO  II  =  1,4 
DO  J J  =  1,4 

T(II,JJ)  =  TIMAT (II, JJ) 

ENDDC 

ENDDO 

C  Manipulator  joint  angles 

THl  =  DTI  +  X(l) 

TH2  =  DT2  +  X(2) 

TK3  =  DT3 

TH4  -  DT4  +  X(4) 

TH5  =  DT5  +  X(5) 

FId  =  DF6  *  X(6) 
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D3  =  DD3  +  X(3) 


C  Compute  the  T  matrices,  T1  thru  T6: 

CALL  t3xyz  (xw, yw, zw, TO ) 

CALL  TRANSFORM  (  ALl,  AAl ,  DDl,  THl,  BLl,  T1  ) 

CALL  TRANSFORM  (  AL2,  AA2,  DD2,  TH2,  BL2,  T2  ) 

CALL  TRANSFORM  (  AL3,  AA3,  D3,  TH3,  BL3,  T3  ) 

CALL  TRANSFORM  (  AL4,  AA4,  DD4,  TK4,  EL4,  T4  ) 

CALL  TRANSFORM  (  AL5,  AA5,  DD5,  TH5,  BL5,  T5  ) 

CALL  t3rpy  {  fi6,  th6,  si6,  trpy  ) 

CALL  T3XYZ  (  PX6,  Pie,  PZ6,  txyz  ) 

CALL  matmulc  (  t6,  trpy,  txyz  ) 

C  Compute  the  overall  transformiation,  T: 

CALL  MATMULA  (  T,  TO  ) 

CALL  MATMULA  {  T,  T1  ) 

CALL  MATMULA  {  T,  T2  ) 

CALL  MATMULA  (  T,  T3  ) 

CALL  MATMULA  (  T,  T4  ) 

CALL  MATMULA  (  T,  T5  ) 

CALL  MATMULA  (  T,  T6  ) 

C  Calculate  the  function  F 

f  (1)  =t  (1,  4)  -tdes  (1,  4) 
f (2) =t (2, 4) -tdes (2,4) 
f(3)=t(3,4)-tdes(3,4) 

RETURN 

END 

Q  it***:ir1t**1rif*****it-k^**-k*it********'k*****ittciritit***’kitit**’k-Ktc*'ic'k**’k*-»*'k*-K**** 

SUBROUTINE  RANDOM  (x,z) 

C  This  subroutine  generates  random  numbers  in  the  range  0-1 
C  using  a  supplied  seed  x,  the  returned  random  number  being  z. 

REAL  FM,  FX,  Z 
INTEGER  A,  X,  I,  M 
DATA  1/1/ 

IF  (  I  .EQ.  0  )  GO  TO  1000 
1  =  0 

M=  2  *  *  2  0 
FM=  M 

A=  2**10  +  3 

lOCC  X=  MOD (  A*X  ,M) 

FX=  X 
Z=  FX/  FM 

RETUPTl 

END 
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PROGRAM  ID 6 

C  Robot  Identification  using  the  Non-linear  Least  Squares  method. 
C  Simulation  data  is  read  for  the  MODEL  G  manipulator  :  rom 
C  the  data  file  TELE-SOLN.DAT 

C  Change  parameter  LDFJAC  to  change  the  number  of  observations, 

C  set  LDFJAC  =  Number  of  observations 


INTEGER  LDFJAC,  MM,  M,  NN,  N,  NSIG,  MAX'^N,  lOPT,  IXJAC,  INFER, 
PARAMETER  (LDFJAC=90,  MM= LDFJAC,  NN=22) 

REAL*8  FJAC (LDFJAC, NN) ,  XJT J ( (NN+ 1 ) *NN/2 ) 

REAL*8  PARM(4),  F (LDFJAC),  WORK ( ( 5 *NN) + ( 2 *MM) + ( (NN+l ) *NN/2 ) ) 
REAL*8  X(NN) 

EXTERNAL  TELE  ARM 


REAL *8 
REAL*8 
REAL *8 

DANGLE,  DLENTH, 
SQERRl,  SQERR2 
XW, YW, ZW 

TQ, 

DQ, 

EPS,  DELTA, 

REAL *8 

DT1,  DT2, 

DT3, 

DT4, 

DT5 

REAL* 8 

DDl,  DD2, 

DD3, 

DD4, 

DD5 

REAL*8 

AAl,  AA2, 

AA3, 

AA4, 

AA5 

REAL* 8 

ALl,  AL2, 

AL3, 

AL4  , 

AL5 

REAL* 8 

BLl,  BL2, 

BL3, 

BL4, 

BL5 

REAL*8 

FI6.  DF6, 

TH6, 

SI6, 

PX6, 

PY6,  PZ6 

INTEGER  I,  J,  K,  NOBS,  MAXNOBS 
REAL*8  magnx,magni 
PARAMETER  ( MAXNOBS= 1 0 0 ) 

REAL*8  TETl (MAXNOBS) ,  TET2 (MAXNOBS ) ,  TET3 (MAXNOBS ) 

REAL*8  TET4 (MAXNOBS) ,  TET5 (MAXNOBS) ,  TET6 (MAXNOBS ) 

REAL* 8  R 

COMMON  /PDATA/  NOBS,  TETl,  TET2,  TET3,  TET4,  TET5,  TET6,  R 


COMMON  /KIN/  DT1,DT2,DT3,BT4,DT5, 
&  ALl, AL2,AL3,AL4,AL5, 

&  ='A1,.AA2,AA3,  AA4,AA5, 

&  DDi,DD2,D.'3,DD4,DD5, 

&  BLl, BL2, BL3, BL4, BL5, 

&  XW, YK, ZK, 


& 

P 

Ff, 

TH6, SI6, PX6, PY6, P26 

Open 

data  ■ 

f  i  les 

f  c 

-r  inputs  and  results 

OPEN 

(8, 

NAME=' RESULT  .DAT' ,  STATUS^ 

OPEN 

(9, 

NAME=' TELE-SOLN.DAT' ,  STA' 

OPEN 

(10, 

NAME=' INPUT.DAT' ,  STATUS= 

Read 

inpat 

pa  ramet 

ers 

read 

(10, 

*) 

read 

(  i  0 , 

*) 

XW, yw, ZW 

read 

(10, 

*) 

dt 1 , ddl ,aal,all,bll 

read 

(10, 

*) 

dt  2 , dd2 , a  a  2 , a  1 2 , b 1 2 

read 

(10, 

*  ) 

dt  3 , dd 3, aa3,al3,bl3 

read 

(10, 

*) 

dt4,dd4,aa4,ai4,bl4 

read 

(10, 

*; 

at  5, dd5, aa5-al5,bl5 

read 

(IC, 

* ) 

6C 


lER 


read  (10,*)  df 6 , th6 , si  6, px6, py6, pz6 
read  '10, *) 

read  (10,*)  nobs,  r,  dangle, dlenth,inagnx, magnl 
CLOSE  (10) 

C  Initialize  data  variables 


X (1) =XW 
X(2) =YK 


X  (3) =DD1 

X  (4) =AA1 
X  (5 ) =AL1 

X (6) =DT2 
X (7) =DD2 
X(8)=AA2 
X(9)=AL2 


X(10) =DD3 
X(11)=AL3 
X (12) =BL3 

X(:3) =DT4 
X(14)=AA4 
X ( 15) =AL4 

X ( 16) =DT5 
X (17) =DD5 
X(18)=AA5 
X(19) =A15 

X (20) =DF6 
X (21) =PX6 
X (22 ) =PZ6 

R=R^MA3NX 

C  Read  simulated  joint  data  and  tool  pose 
READ  (9,*)  NOBS 
DC  J  =  1,  NOBS 

REAX)  (9,*)  TETl(J),  TET2  (J)  ,  TET3  ( J)  ,  TET4(J),  TET5  ( J)  , 
ENDDO 
CLOSE  (9) 

C  Call  IMSL  routine  for  non-linear  identification 

NS  10=4 
EPS  =  C  .  0 
DELTA=C . C 
MAXFN=15CC 
ICPT=: 

:XJAC=LDFJAC 

K=NOBS 

CALL  ZXSSQ (TELE_ARN,N, NN, NS 13, EPS, DELTA, MAXFN, lOPT, 

&  PARv, X, SSQ,F,FJAC, IXJAC, X JTJ, WORK, INFER,  lER) 


TET6 ( J) 
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C  Save  results  to  data  file 


WRITE 

(8, 

*) 

WRITE 

(8, 

*) 

'  XW, 

YW,  ZW' 

WRITE 

(8, 

*) 

X(l)  , 

X(2) 

,  ZW 

WRITE 

(8, 

*) 

WRITE 

(8, 

*) 

'DTI, 

DDl, 

AAl, 

ALl,  BLl' 

WRITE 

(8, 

*) 

0.0, 

X(3)  , 

X(4)  , 

X(5),  0.0 

WRITE 

(8, 

*) 

WRITE 

(8, 

*) 

'  DT2, 

DD2, 

AA2, 

AL2,  BL2' 

WRITE 

(8, 

*) 

X(6)  , 

X(7) 

,  X(8) 

,  X(9),  0.0 

WRITE 

(8, 

*) 

WRITE 

(8, 

*) 

'DT3, 

DD3, 

AA3, 

AL3,  BL3' 

WRITE 

(8, 

*) 

0.0, 

X(10) 

,  0.0, 

X(I1),  X(12) 

WRITE 

(8, 

*) 

WRITE 

(8, 

*) 

'  DT4, 

DD4, 

AA4, 

AL4,  BL4' 

WRITE 

(8, 

*) 

X(I3) 

,  0.0 

,  X(14),  X(15),  0 

.0 

WRITE 

(8, 

*) 

WRITE 

(8, 

*) 

'  DT5, 

DD5, 

AA5, 

AL5,  BL5' 

WRITE 

(8, 

*) 

X(16) 

,  X(17),  X(18),  X(19), 

0, 

.0 

WRITE 

(8, 

*) 

WRITE 

(8, 

*) 

'  DF6 

,  TH6 

,  SI6, 

PX6,  PY6, 

PZ6,  R' 

WRITE 

(8, 

*) 

X (20) ,  0  . 

o 

o 

o 

,  X(21),  0. 

0, 

X(22) 

c  Restore  initial  values  of  input  parameters 
open  (10, name=' input .dat' , status='old' ) 


read 

(10, *) 

read 

(10, *) 

XW, yw, ZW 

read 

(10, *) 

dtl,ddl,aal,all,bll 

read 

(10, *) 

dt2, dd2, aa2,al2,bl2 

read 

(10,  *) 

dt3,dd3,aa3,al3,bl3 

read 

(10, *) 

dt  4 , dd4 , aa4 , al4, bl4 

read 

(10, *) 

dt5,dd5,aa5,al5,bi5 

read 

(10, *) 

read 

(10, *) 

df  6, th6, si6, px6, py6, pz6 

read 

(10, *) 

read 

(10, *) 

nobs , r , dangle, dlenth,magnx,  magnl 

CLOSE 

(10) 

C  Calculate  root  mean  square  error  in  identification 

TQ  =  DANGLE 
DQ  =  DLENTH 


C  Error  in  identification  (angular  parameters) 

SQERRl  = 

&  (ALl+TQ-X (5) ) **2  + (DT2+TQ-X(6) ) **2  + ( AL2+TQ-X ( 9 ) ) * *2 
&  +(AL3+TQ-X(11) ) **2 

6.  +  (BL3+TQ-X(12)  )  **2  +  (DT4+TQ-X(13)  )  **2 

&  + (AL4+TQ-X (15) ) **2  + (DT5+TQ-X(16) ) **2 

&  + (AL5+TQ-X (19) ) **2 

&  + (DF6+TQ-X(20) ) **2 

SQERRl  =  rSQRT  (  SQERRl/10  ) 

C  Error  in  identification  (length  parameters) 

SQERR2  = 
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&  (DDl+DQ-X (3) ) **2  + (AA1+DQ-X(4) ) **2 
&  + (DD2+DQ-X (7) ) **2  + (AA2+DQ-X (8 ) ) **2 

&  + (DD3+DQ-X (10) ) **2  + (AA4+DQ-X(14) ) **2 

&  +  (DD5+DQ-X(17) ) **2  + (AA5+DQ-X ( 18 ) ) * *2 

&  + (PX6+DQ-X (21) ) **2  + (PZ6+DQ-X (22) ) **2 

&  + (xw+dq-x ( 1 ) ) **2  + (yw+dq-x (2) ) **2 

SQERR2  =  DSQRT(  SQERR2/12  ) 

WRITE  (8,*) 

WRITE  (8,*)  'RMS  PARMS  (LENGTH),  RMS  FARMS  (ANGLE)' 
WRITE  (8,*)  SQERR2,  SQERRl 

WRITE  (6,*)  'RMS  PARMS  (LENGTH),  RMS  PARMS  (ANGLE)' 
WRITE  (6,*)  SQERR2, SQERRl 


WRITE  (8,*) 

WRITE  (8,*)  'INFER,  lER, NOBS, NSIG' 

WRITE  (8,*)  INFER,  lER, NOBS , NS IG 
WRITE  (6,*)  'INFER,  lER, NOBS, NSIG' 

WRITE  (6,*)  INFER,  lER, NOBS, NSIG 
WRITE  (8,*) 

CLOSE  (8) 

END 

0  ilr'<r***:#r*T»r***A'^?<t-**r**)t***-Jkr******iir***********A*jtTt***n*****T^Tt*Tt*Tt******<rTtTk 

SUBROUTINE  TELE_ARM  (X,  M,  N,  F) 

C  This  subroutine  calculates  the  non-linear  function  for  the  use  of 
C  the  IMSL  routine  ZXSSQ.  It  is  the  forward  Icinematic  solution  for 
C  the  MODEL  G  manipulator. 

INTEGER  M,  N 
REAL’S  X(N),  F(M) 

INTEGER  II,  JJ 


REAL’S  XW,  YW,  ZW 


REAL’S 

DTI, 

DT2, 

DT3, 

DT4, 

DT5 

REAL’S 

DDl, 

DD2, 

DD3, 

DD4, 

DD5 

REAL’S 

AAl, 

AA2, 

AA3, 

AA4, 

AA5 

REAL’S 

ALl, 

AL2, 

AL3, 

AL4, 

AL5 

REAL’S 

BLl , 

BL2, 

BL3, 

BL4, 

BL5 

REAL’S 

f  i  6 , 

df  6, 

th6. 

si6. 

PX6,  PY6,  PZ6,  D3 

REAL’S 

THl, 

TH2, 

TH3, 

TH4, 

TH5 

REAL’S 

TO  (4 

,4)  , 

T1  (4, 

4),  T2(4,4),  T3(4,4),  T4(4,4) 

REAL’S  T5(4,4),  T6(4,4),  trpy(4,4),  txyz(4,4) 

REAL’S  TIMAT(4,4),  T(4,4) 

INTEGER  I,  J,  K,  NOBS,  MAXNOBS 
PARAMETER  (MAXNOBS=l 0 0 ) 

REAL’S  TETl (MAXNOBS) ,  TET 2 (MAXNOBS ) ,  TET3 (MAXNOBS ) 

REAL’S  TET 4 (MAXNOBS) ,  TET5 (MAXNOBS ) ,  TET6 (MAXNOBS ) 

REAL’S  R,  RR 

COMMON  /PDATA/  NOBS,  TETl,  TET2 ,  TET3,  TET4,  TET5,  TET6,  R 

COMMON  /KIN/  DT1,DT2,DT3,DT4,DT5, 

&  ALl, AL2,AL3,AL4,AL5, 
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&  AAl , AA2 , AA3, AA4 , AA5, 

&  DD1,DD2,DD3,DD4,DD5, 

&  BL1,BL2,BL3,BL4,BL5, 

&  XW,YW,ZW, 

&  DF6,TH6,SI6,PX6,PY6,PZ6 

C  Initialize  the  TIMAT  matrix  to  an  I  matrix; 

DATA  TIMAT/ 1, 0,0,0,0,1,0,0,0,0,1,0,0,0,0,17 

C  Set  parameters  for  the  manipulator: 


xw  = 

X(l) 

YW  = 

X(2) 

DDl  = 

X(3) 

AAl  = 

X(4) 

ALl  = 

X  (5) 

DT2  = 

X(6) 

DD2  = 

X(7) 

AA2  = 

X(8) 

AL2  = 

X(9) 

DD3  = 

X(10) 

AL3  = 

X(ll) 

BL3  = 

X(12) 

DT4  = 

X(13) 

AA4  = 

X{14) 

AL4  = 

X(15) 

DT5  = 

X(16) 

DD5  = 

X(17) 

AA5  =  X(18) 

AL5  =  X(19) 

DF6  =  X(20) 

PX6  =  X(21) 

PZ6  =  X(22) 

C  Loop  NOBS  times 

K  =  0 

DO  J  =  1,  NOBS 

C  Initialize  the  7  matrix  to  an  I  matrix 

DO  II  =  1,4 
DO  JJ  =  1,4 

T(II,JJ)  =  TIMAT(II,JJ) 

ENDDO 

ENDDO 

C  Manipulator  joint  angles 


THl  =  DTI 

TETl ( J) 

TH2  =  DT2 
TH3  =  DT3 

+ 

TET2 ( J) 

TH4  =  DT4 

4- 

TET4 ( J) 

TH5  =  DT5 

4 

TET5 ( J) 

A 


4 
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FI6  =  DF6  +  TET6 ( J) 

D3  =  DD3  +  TET3 ( J) 

C  Compute  the  T  matrices,  T1  thru  T6: 

CALL  T3XYZ  (XW, YW, ZW, TO ) 

CALL  TRANSFORM  (  ALl,  AAl ,  DDL,  THl,  BLl,  T1  ) 

CALL  TRANSFORM  (  AL2 ,  AA2,  DD2,  TH2,  BL2,  T2  ) 

CALL  TRANSFOFIM  (  AL3,  AA3,  D3,  TH3,  BL3,  T3  ) 

CALL  TRANSFORM  (  AL4,  AA4,  DD4,  TH4,  BL4,  T4  ) 

CALL  TRANSFORM  (  AL5,  AA5,  DD5,  TH5,  BL5,  T5  ) 

CALL  t3rpy  (  fi6,  th6,  si6,  trpy  ) 

CALL  T3XYZ  (  PX6,  PY6,  PZ6,  txyz  ) 

CALL  matmulc  (  t6,  trpy,  txyz  ) 

C  Compute  the  overall  transformation,  T: 

CALL  MATMULA  (  T,  TO  ) 

CALL  MATMULA  (  T,  T1  ) 

CALL  MATMULA  (  T,  T2  ) 

CALL  MATMULA  (  T,  T3  ) 

CALL  MATMULA  (  T,  T4  ) 

CALL  MATMULA  (  T,  T5  ) 

CALL  MATMULA  (  T,  T6  ) 

C  Calculate  the  function  F 

rr=dsqrt  (  t  ( 1 ,  4  )  *t  ( 1 ,  4 ) +t  (2 , 4 )  *t  (2,  4 ) +t  ( 3,  4 )  *t  ( 3 ,  4 )  ) 
f { j ) =dabs (  rr-r ) 

C  End  the  do-loop  for  counter  J 
ENDDO 

C  Compute  RMS  error 

sumsq=0 . 0 
do  j=l,  nobs 
sumsq=sumsq+f ( j )  *f  {  j  ) 
enddo 

rms=sqrt (sumsq/nobs) 
write  ( 6 , * )  rms 

RETURN 

END 
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PROGRAM  VERIFY 

C  This  program  generates  the  six-dof  pose  error  for  the  MODEL  G 
manipulator . 

C  It  contains  the  identified  calibration  parameters  and  the  exact 
parameter . 

C  It  uses  a  data  file  of  verification  joint  angle  sets  POSEVER.DAT,  and 
the 

C  file  RESULT. DAT  fromi  the  program  ID6. 

INTEGER  I,  J,  K,  NPOSES,  N 
REAL* 8  DANGLE,  DLENTH 

REAL* 8  P(200),OR(200),W1(200),W2(200),W3(200) 

REAL*8  DT (5)  ,  dd (5) , aa (5) , al (5) ,bl (5) ,  world(3) 

REAL*8  eDT ( 5 ) , edd ( 5 ) , eaa ( 5 ) , eal ( 5 ) , ebl ( 5 ) ,  eworld(3)  * 

REAL*8  edf6,  EFI6,  ETHc,  ESI6,  EPX6,  EPY6,  EPZ6 
REAL*8  THETAdOOO,  6)  ,  TDELTA(4,4) 

REAL*8  T0(4,4),  Tl(4,4),  T2(4,4),  T3(4,4) 

REAL*8  T4(4,4),  T5(4,4),  T6(4,4),  TRPY(4,4),  TXYZ(4,4) 

REAL*8  TIMAT(4,4),  T(4,4),  et(4,4) 


REAL* 8 

P  'T' 

f  ^  f 

rs**-  / 

.4  T  / 

-  'T>  C 

REAL*8 

DDl, 

DD2 

,  DD3, 

DD4, 

DD5 

REAL *8 

AAl, 

AA2 

,  AA3, 

AA4, 

AA5 

REAL*8 

ALl, 

AL2 

,  AL3, 

AL4, 

AL5 

REAL*8 

BLl, 

EL2 

,  BL3, 

BL4, 

BLo 

REAL*e 

DF6, 

FI6 

,  TH6, 

Sit, 

PXr ,  PYc,  PZ6 

REAL*  8 

XW, 

YW, 

ZVv 

COMMON 

TIMA 

* 

HETA 

itialize  the 

'  ^  M. 

MA,T  m.a  t 

r  .  .X 

1 1  an  1  ma t  r  i : 

DATA  T 

I  MAT/ 

i.  ,  ^ 

,  C  ,  w  ,  , 

^  t  ^  t  ■J  t  \J  f  \J  f  i 

C  •  pen  data  file 

OPEN  (9,  NAME=' pcsever -DAT' , STATUS=' OLD' ) 
OPEN  (10,  NAME=' input .DAT' ,  STATUS=' OLD' ) 
OPEN  (11,  NAME  =  '  result  .DAT' ,  STATUS  =  ' OLD' ) 

c  'ead  input  parameters 

read  (10,*) 
read  (1C, * ) 
read  (1C, * ) 
read  (10, * ) 
read  (1C, * ) 
read  (10, * ) 
read  (10, * ) 
read  (1C, * ) 
read  (10, * ) 
read  (10, * ) 
read  (10, * ) 

CLOSE  (1C) 


c  Read  joint  angle  sets  for  verificaticn  poses 
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read  ( 9 , * )  nposes 


do  i=l, nposes 

read (9, *)theta (i, 1) , theta (i,2) , theta ( i, 3) , theta ( i,  4 )  , 
&  theta (i, 5) , theta (i, 6) 

enddc 
close  (9) 

C  Set  exact  link  parameters  for  the  manipulator: 

dt(l)  =  dtl  !  defined 

dt(2)  =  dt2  dangle 
dt(3)  =  dt3  !  defined 

dt { 4 )  =  dt 4  +  dangle 
dt ( 5 )  =  dtS  +  dangle 

world(l)  =  world (1)  +  dlenth 
world(2)  =  world(2)  +  dlenth 


al (1)  =  all 

DANGLE 

a  1  ( 2  )  •■=  a  1 2 

DANGLE 

al  (  3 )  =  al 3 

+ 

DANGLE 

al  (4)  =  al4 

4- 

DANGLE 

a  1  ( 5 )  =  al 5 

4- 

DANGLE 

AA(1)  =  aal 

■+■ 

DLENTH 

AA(2)  =  aa2 

+ 

DLENTH 

AA(3)  =  aa3 

!  defined 

AA(4)  =  aa4 

-i- 

DLENTH 

AA(5)  =  aa5 

4- 

DLENTH 

DD(1)  =  ddl 

+ 

DLENTH 

DD(2)  =  dd2 

4. 

DLENTH 

DD(3)  =  dd3 

+ 

DLENTH 

DD(4)  =  dd4 

I  defined 

DD (5)  =  ddS 

DLENTH 

BL(1)  =  bll 

1  defined 

BL(2)  =  bl2 

!  defined 

BL(3)  =  bl3 

4- 

DANGLE 

BL (4)  =  tl4 

!  defined 

BL  (5)  =  bl'i 

'  defined 

DF 6  =  DF6  -r 

0, 

angle 

TK6  =  0. 

r\ 

S 1 6  =  C  . 

V 

PXc  =  PX6  - 

LENTH 

PY6  =  C. 

P26  =  PZ6  - 

DLENTH 

c  Read  in  and  set  up  estimated  parameter  table 

read (11, * ) 
read (11,  *  > 

read(ll, *)  ewcrld(l) ,ewcrld(2) , ewor Id  ( 3 ) 

dc  i=  1 ,  5 
read  (11,*) 
read  (11, *) 

read  (11,*)  edt ( i ) , edd ( i ) , eaa ( i ) , eal (i) , ebl ( 1 ) 
enddc 
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read (11, *; 
read (11, * ) 

read ( 1 1 , * )  edf  6, et h6 , esi 6 , epx6, epy6, epz6,  r 
c  do  kk=l , 3 

c  write ( 6, *) world (kk) , eWorld (kk) 
c  enddo 

c  do  ii=l , 6 

c  write ( 6,  *  )  ii 

c  write(6,*)al(ii),eal(ii),aa(ii),eaa(ii),dd(ii), edd ( ii )  , 

c  &  bl ( ii ) , ebl ( ii ) , dt ( ii ) , edt (ii) 

o  enddo 


c  Main  loop  through  NPOSES  joint  angle  sets 
do  k=l,nposes 

call  fks  ( k, wor Id, dt , al , aa , dd, bl, f i6, th6, si 6, px6 , py6 , pz6,  t ) 

call  fks  ( k, eWorld, edt , eal , eaa, edd, ebl , ef i 6 , eth6 , es i6 , epx6 , 
&  epy6 , epz6 , et ) 

c  Compute  the  differential  tool  matrix 

call  matsub  ( tde Ita , t , et ) 

c  Cor.oute  the  pose  errors 

poserr=sqrt (tdelta (1, 4) **2+tdelta (2,4)**2+tdelta(3,4)**2) 

orerrl=(tdelta(3,2)-tdelta(2,3))/2 

orerr2=(tdelta(l,3)-tdelta(3,l))/2 

orerr3=(tdeita(2,l)-tdelta(l,2))/2 

orerr=sqrt(orerrl**2+orerr2**2+orerr3**2) 

c  Update  total  error  counts 

posterr=(poserr+(k-l) *posterr) /k 
orterr  =(orerr  (k-1)  *orterr) /k 

c  End  of  main  loop 


enddc 


write  (6,*)  'Position  error,  orientation  error' 
write  (6,*)  poster r, orterr 

OPEN  (19,  NAME=' VER.DAT' ,  STATUS =' OLD' ) 

READ  (19,*)  NR 

IF(NR.GT.O)  READ;19,*)  (P  ( I )  ,  OR  ( I )  ,  W1  ( I )  ,  W2  ( I )  ,  W3  ( I )  ,  1  =  1 ,  NR)  -« 

NR=NR^1 

p  (nr ) =POSTERR 

or  (nr) =ORTERR  ^ 

wl (nr) =WORLD (1) -DLENTH 
w2 (nr) =WORLD (2) -DLENTK 
w3 (nr) =WORLD (3) 

REWIND  19 
WRITE (19, *)  nr 

WRITE (19, *)  (P  (I)  ,OR(I) ,W1 (I) ,W2 (I) ,W3 (1) , 1  =  1,  NR) 
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CLOSE  (19) 


end 


subroutine  fks  (n, wor Id, dt , al, aa, dd, bl , df 6 , th6 , si6 , 

&  px6, py6, pz6, t) 

REAL*8  T0{4,4),  Tl(4,4),  T2(4,4),  T3(4,4) 

REALMS  T4(4,4),  T5(4,4),  T6(4,4),  TRPY(4,4),  TXYZ(4,4) 
REAL*8  TIMAT(4,4),  T(4,4),  dt ( 5 ) , a 1 ( 5 ) , aa ( 5 )  ,  dd ( 5 )  ,  bl( 5 ) 
real*8  theta ( 1 0 0 0 ,  6 )  ,  ang{5),  world(3) 
common  timat, theta 

C  Initialize  the  T  matrix  to  an  I  matrix: 

DO  J=l,  4 
DO  K=l, 4 

T(J,K)  =  TIMAT(J,K) 

ENDDO 

EKDDO 


C  Set  up  the  joint  angles 
do  i=l,5 

ang ( i ) =theta (n, i ) 
enddo 


fi6=theta (n, 6) +df6 


C  Compute 
call 


the  T  m.atrices,  T1  thru  T6: 

t3xyz  (world(l) ,world(2) ,world(3) ,T0) 


CALL  TRANSFORM  ( a  1 ( 1 ) , aa ( 1 ) , dd < 1 ) , ang ( 1 ) , bl ( 1) , T 1) 
CALL  TRANSFORM  ( a 1 ( 2 ) , aa ( 2 ) , dd ( 2 ) , ang (2 ) , bl (2 ) , T2 ) 
CALL  TRANSFORM  ( a  1 ( 3 ) , aa ( 3 ) , ang ( 3 ) , dt ( 3 ) , bl ( 3 ) , T3 ) 
CALL  TRANSFORM  ( a  1 ( 4 ) , aa ( 4 ) , dd ( 4 ) , ang ( 4 ) , bl ( 4 ) , T4 ) 
CALL  TRANSFORM  ( a  1 ( 5 ) , aa ( 5 ) , dd ( 5 ) , ang ( 5 ) , bl ( 5 ) , T5 ) 

CALL  T3RPY  ( f i 6 , t h 6 , s i 6 , TRPY  ) 

CALL  T3XYZ  (px6, py6, pz6, TXYZ  ) 


CALL 

MATMULC 

(  T6 

,  TRPY,  TXYZ  ) 

Compute 

the  overall 

t  ransf  orm^ation 

CALL 

MATMULA 

(  T, 

TO  ) 

CALL 

MATMULA 

(  T, 

T1  ) 

CALL 

MATMULA 

(  1, 

T2  ) 

CALL 

MATMULA 

(  T, 

T3  ) 

CALL 

MATMULA 

(  T, 

T4  ) 

CALL 

MJ^TMULA 

(  T, 

T5  ) 

CALL 

MATMULA 

(  T, 

T6  ) 

return 

end 
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